#pragma config(Hubs,  S1, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motor2,        tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C1_2,     motor4,        tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     motor3,        tmotorTetrix, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_2,     motor1,        tmotorTetrix, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"

void initializeRobot()
{

	return;
}


task main()
{
	initializeRobot();

	waitForStart(); // wait for start of tele-op phase

	int x1Val = 0; // these four are all just for easy manipulation of
	int y1Val = 0; // joystick values from gamepad one.
	int x2Val = 0; // the data from pad 2 is used less intensively,
	int y2Val = 0; // so these are un-needed there.

	int boundBox = 20; // size of the bounding box for the wheels

	int scale = 5; // the amount by which the wheels are slowed by default.
	// With a button the bring it up to full speed

	int motor1Val = 0; // same as the joystick values, just to make life easier
	int motor2Val = 0;
	int motor3Val = 0;
	int motor4Val = 0;

	int eleVal = 0; // and once again, these two are just to make dealing
	int clawVal = 0; // with the numbers simpler

	/*
	int eleSpeed = 50;
	int clawSpeed = 20;
	*/

	while (true)
	{
		getJoystickSettings(joystick);

		/*------------------------Joystick Values-------------------------*\
		||
		|| Getting a bounding box/cutting down sensitivity from joysticks
		||
		\*----------------------------------------------------------------*/

		if(abs(joystick.joy1_x1) > boundBox)
		{
			x1Val = joystick.joy1_x1 / scale;
		}
		else
		{
			x1Val = 0;
		}

		if(abs(joystick.joy1_y1) > boundBox)
		{
			y1Val = joystick.joy1_y1 / scale;
		}
		else
		{
			y1Val = 0;
		}

		if(abs(joystick.joy1_x2) > boundBox)
		{
			x2Val = joystick.joy1_x2 / scale;
		}
		else
		{
			x2Val = 0;
		}

		if(abs(joystick.joy1_y2) > boundBox)
		{
			y2Val = joystick.joy1_y2 / scale;
		}
		else
		{
			y2Val = 0;
		}


		/*--------------------Holonomic Motor Assignments-------------------*\
		|| OVERVIEW:
		||
		|| Alright, this is the tricky bit of the holonomic system. The
		|| "y1Val" and "x1Val" together make up the "strafing" movement, while
		|| the "x2Val" is rotation. The tough bit comes because when driving,
		|| each motor is paired with another in each axis(x and y). These
		|| pairings are different for foreward and backward movement versus
		|| sideways movement. As you can see, the pairings in the x-axis are
		|| 1-2 and 3-4, but in the y-axis they are 1-3 and 2-4. This is key,
		|| and is due to the fact that when driving LEFT, the FRONT two
		|| wheels go the one direction and the BACK two must go the other,
		|| while when driving FORWEARD, the LEFT two wheels must go one
		|| direction, and the RIGHT must go the other.
		||
		||----------------------------------------------------------------------
		|| MOTOR CONNECTIONS:
		||
		|| This part is very much a pain to reverse engineer blindly, so be
		|| sure this is the exact setup.
		||
		|| front left = motor1
		|| front right = motor2
		|| back left = motor3
		|| back right = motor4
		||
		|| Note: it is entirely fine to make any side of your robot the
		|| "front." Being holonomic makes front, back, left, and right largely
		|| arbitrary anyway. The key part of the motor connections is the
		|| relative positions of the motors on the robot(i.e. 1 and 2 are
		|| adjacent, 3 is diagonally across from 2, and 4 is adjacent to 3).
		||
		|| For example, the two setups below are equally vaild:
		||
		|| 1 /---------\ 2
		|| | |
		|| | |
		|| | |
		|| | |
		|| 3 \---------/ 4
		||
		||
		|| 2 /---------\ 4
		|| | |
		|| | |
		|| | |
		|| | |
		|| 1 \---------/ 3
		||
		|| The relative positions are the same in each case.
		||
		||
		||
		|| In case of problems, check to ensure that either ALL, or NONE of
		|| the motors are reversed in the "Motors and Sensors Setup" window
		|| (assuming that all wheels are geared the same).
		||
		\*-------------------------------------------------------------------*/

		motor1Val = -y1Val - x1Val - x2Val;
		motor2Val = y1Val - x1Val - x2Val;
		motor3Val = -y1Val + x1Val - x2Val;
		motor4Val = y1Val + x1Val - x2Val;
		// I know, that was a lot of build up for just four little lines. But
		// those four lines are the heart and soul of the holonomic drive.


		//speed boost (up to full speed)
		if(joy1Btn(5))
		{
			motor1Val = motor1Val * scale;
			motor2Val = motor2Val * scale;
			motor3Val = motor3Val * scale;
			motor4Val = motor4Val * scale;
		}






		//and finally, we take the calculated values and give them to the
		//motors.
		motor[motor1] = motor1Val;
		motor[motor2] = motor2Val;
		motor[motor3] = motor3Val;
		motor[motor4] = motor4Val;

	}//end while
}//end main
